#include <stdio.h>
#include <string.h>
#include <alloca.h>


int sort_binary_search(void* p, size_t size1, int n, void* k, 
        int (*cmp)(const void* s1, const void* s2, void* arg), void* cmp_arg)
{
    #define a(idx)  (p+size1*(idx))
    int retidx = 0;
    int from = 0;
    int to = n-1;
    int m;
    while(1) {
        if(0 == from-to) {
            retidx = cmp(k, a(from), cmp_arg)>=0?from+1:from;
            break;
        }

        if(1 == from-to) {
            if(cmp(k, a(from), cmp_arg)<0) {
                retidx = from;
            }
            else if(cmp(k, a(to), cmp_arg)<0) {
                retidx = to;
            }
            else {
                retidx = to+1;
            }

            break;
        }

        m = from + (to-from)/2 ;
        if(0 == cmp(k ,a(m), cmp_arg)) {
            retidx = m+1;
            while(0 == cmp(k, a(retidx), cmp_arg)) {
                retidx ++;
            }

            break;
        }
        else if(cmp(k, a(m), cmp_arg)>0) {
            from = m + 1;
        }
        else {
            to = m - 1 ;
        }
    }
    #undef a

    return retidx;
}


int sort_binary_search_insert(void* p, size_t size1, int n, 
        int (*cmp)(const void* s1, const void* s2, void* arg), void* cmp_arg)
{
    int idx = 0;
    int i;
    int j;
    void* k_add = alloca(size1);

    for(i=1; i<n; i++) {
        memcpy(k_add, p+size1*i, size1);

        idx =  sort_binary_search(p, size1, i, k_add, cmp, cmp_arg);

        for(j=i-1; j>=idx; j--) {
            memcpy(p+size1*(j+1), p+size1*j, size1);
        }

        memcpy(p+size1*idx, k_add, size1);
    }

    return 0;
}


void display(int a[], int n)
{
    int i;
    for(i=0; i<n; i++) {
        printf("%d ", a[i]);
    }
    printf("\n");
}


int intcmp(const void* s1, const void* s2, void* arg)
{
    return *(int*)s1 - *(int*)s2;


}


int main()
{
    int a[] = {1,6,2,3,9,7,8,0,4,5,1,6,2,3,9,7,8,0,4,5};
    int n = sizeof(a)/sizeof(a[0]);
    sort_binary_search_insert(a, sizeof(int), n, intcmp, NULL);
    display(a, n);

    return 0;
}
